/*
 * Defined here is the N-dimensional matrix class, designed to
 * cater for all your vector needs. Behold.
 *
 * Authors: Sean, Dan 
 * Magicness rating: 10/10
 **/

#ifndef __vectorH__
#define __vectorH__

#include <math.h>
#include "string.h"
#include <assert.h>
#include "meta.h"


//// Forward declarations

template <class T, unsigned M, unsigned N> struct MatrixOps;
template <class T, unsigned Dimension> struct VectorOps;

template <class T, unsigned Dimension, class Ops> struct Vector;
template <class T, unsigned M, unsigned N, class Ops> struct Matrix;

/// Interface for using the matrix class alone
template <class T, unsigned M, unsigned N> struct MatrixType {
	typedef Matrix<T, M, N, MatrixOps<T, M, N> > tType;
};
template <class T, unsigned Dimension> struct VectorType {
	typedef Vector<T, Dimension, VectorOps<T, Dimension> > tType;
};



//// Matrix

/// Wrapper for matrix elements
template <class T>
struct Element {
	/// Base element type
	typedef typename Meta::ValueTraits<T>::tStorage tType;
	typedef typename Meta::ValueTraits<T>::tRef tTypeRef;
	typedef typename Meta::ValueTraits<T>::tConstRef tConstTypeRef;

	/// Element storage
	tType Value;

	inline Element<T>(): Value() {};
	inline Element<T>(const T& v): Value(v) {};
	
	inline void operator =(const Element<T>& e) {
		Value = e.Value;
	};
	inline void operator =(tConstTypeRef v) {
		Value = v;
	};

	inline const Element<T>& operator [](int i) const {
		assert(i == 0);
		return *this;
	};
	inline Element<T>& operator [](int i) {
		assert(i == 0);
		return *this;
	};

	inline operator tConstTypeRef () const {
		return Value;
	};
	inline operator tTypeRef () {
		return Value;
	};

	// Delegate standard operators to the base type
	DELEGATE_OPERATORS_IN_TPL(T, Value);
};

template <class T, unsigned M, unsigned N> struct MatrixOps {
	typedef MatrixOps<T, M, N> tThisType;
	typedef Matrix<T, M, N, tThisType> tMatrix;

	/// Wrapper for gettting the type of Value and columns
	template<unsigned _M> struct Component {
		typedef typename VectorType<T, _M>::tType tType;
	};
	/// Specialisation for vectors (take primitive type)
	template<> struct Component<1> {
		typedef T tType;
	};

	/// For producing result matrices of the same type, but (possibly)
	/// differing dimensions
	template<unsigned _M, unsigned _N>
	struct Redim {
		typedef Matrix<T, _M, _N, MatrixOps<T, _M, _N> > result;
	};
	/// Single-column matrices returned as a vector
	template<unsigned Dimension>
	struct Redim<Dimension, 1> {
		typedef Vector<T, Dimension, VectorOps<T, Dimension> > result;
	};
	/// Singleton matrices returned as an element
	template<>
	struct Redim<1, 1> {
		typedef typename Component<1>::tType result;
	};
};

/// Traits for scalar types
template <class MT>
struct MatrixTraits {
	typedef MT tMatrixType;
	typedef MT tElement;
	typedef Meta::Null Ops;
	typedef MT tRow;
	typedef MT tCol;

	static unsigned const rows = 1;
	static unsigned const columns = 1;

	/// Allow addressing of scalar types
	inline static MT& RowOf(MT& m, unsigned i) {
		assert(i == 0);
		return m;
	};
	inline static const MT& RowOf(const MT& m, unsigned i) {
		assert(i == 0);
		return m;
	};
};

template <class T, unsigned M, unsigned N, class Ops>
struct MatrixTraits<Matrix<T,M,N,Ops> > {
	typedef Matrix<T,M,N,Ops> tMatrixType;
	typedef T tElement;
	typedef Ops Ops;
	typedef typename Ops::Component<N>::tType tRow;
	typedef typename Ops::Component<M>::tType tCol;

	static unsigned const rows = M;
	static unsigned const columns = N;

	/// Addressing of matrix types
	inline static tRow& RowOf(tMatrixType& m, unsigned i) {
		assert(i < M);
		return m[i];
	};
	inline static const tRow& RowOf(const tMatrixType& m, unsigned i) {
		assert(i < M);
		return m[i];
	};
};

/**
 * The matrix class
 **/
template <class T, unsigned M, unsigned N, class Ops>
class Matrix
{
public:
	typedef Matrix<T,M,N,Ops> tThis;
	typedef typename Ops::tMatrix tThisMatrix;

	/// Common redimensions
	typedef typename Ops::Redim<N, M>::result tTranspose;
	typedef typename Ops::Component<N>::tType tRow;
	typedef typename Ops::Component<M>::tType tCol;

private:
	/// The row storage
	tRow Value[M];

public:
	/// Constructor, initialized to zero
	inline Matrix<T,M,N,Ops>(){
		for (int i = 0; i < M; i++) {
			Value[i] = tRow();
		};
	};
	/// Copy constructor
	inline Matrix<T,M,N,Ops>(const tThis& m){
		for (int i = 0; i < M; i++) {
			Value[i] = m[i];
		};
	};

	/// Implicit conversion from other matrix types. Allows initialization from
	/// other matrix sizes - unfilled rows are zeroed.
	template <class _T, unsigned _M, unsigned _N, class _Ops>
	inline Matrix<T,M,N,Ops>(const Matrix<_T,_M,_N,_Ops>& v) {
		const unsigned lim = (M < _M ? M : _M);
		int i;
		for(i = 0; i < lim; i++) {
			(*this)[i] = v[i];
		};
		for(; i < M; i++) {
			(*this)[i] = tRow();
		};
	};

	/// Matrix addition
	inline tThisMatrix operator +(const tThis& a) const{
		tThisMatrix b(*this);
		b += a;
		return b;
	};
	inline void operator +=(const tThis& a){
		for(int i = 0; i < M; i++) {
			Value[i] += a[i];
		};
	};

	/// Matrix subtraction
	inline tThisMatrix operator -(const tThis& a) const{
		tThisMatrix b(*this);
		b -= a;
		return b;
	};
	inline void operator -=(const tThis& a){
		for(int i = 0; i < M; i++) {
			Value[i] -= a[i];
		};
	};


	/// Implementation of scalar multiplication
	template <class T2>
	struct _Mult {
		typedef tThisMatrix tRet;

		inline static tRet Eval(const tThis& Value, const T2& a) {
			tRet b;
			for(unsigned i = 0; i < M; i++) for(unsigned j = 0; j < N; j++){
				MatrixTraits<tRet::tRow>::RowOf(b[i],j)
					= MatrixTraits<tThisMatrix::tRow>::RowOf(Value[i],j) * a;
			};
			return b;
		};
	};

	/// Implementation of matrix multiplication
	template <class _T, unsigned _P, class _Ops>
	struct _Mult<Matrix<_T, N, _P, _Ops> > {
		typedef Matrix<_T, N, _P, _Ops> tMatrix;
		typedef typename Ops::Redim<M, _P>::result tRet;
		typedef typename MatrixTraits<tRet> tRetTraits;
		typedef typename MatrixTraits<typename tRetTraits::tRow> tRetRowTraits;

		inline static tRet Eval(const tThis& Value, const tMatrix& a) {
			tRet b;
			for(unsigned i = 0; i < M; i++){
				for(unsigned j = 0; j < _P; j++){
					tRetRowTraits::RowOf(tRetTraits::RowOf(b,i),j) = 0;
					for(unsigned k = 0; k < N; k++){
						// We know a is addressable, but its rows may be singleton elements,
						// so access using the traits of a's row type
						tRetRowTraits::RowOf(tRetTraits::RowOf(b,i),j)
							+= MatrixTraits<tThisMatrix::tRow>::RowOf(Value[i],k) * MatrixTraits<tMatrix::tRow>::RowOf(a[k],j);
					};
				};
			};
			return b;
		};
	};

	/// Generic Multiplicication (takes scalars and matrices)
	template <class MT>
	inline typename _Mult<typename MatrixTraits<MT>::tMatrixType>::tRet
	operator * (const MT& a) const {
		return typename _Mult<typename MatrixTraits<MT>::tMatrixType>::Eval(*this, a);
	};
	template <class MT>
	inline void operator *= (const MT& a) {
		*this = typename _Mult<typename MatrixTraits<MT>::tMatrixType>::Eval(*this, a);
	};

	/// Scalar division
	inline tThisMatrix operator / (const T& v) const {
		tThisMatrix m(*this);
		m /= v;
		return m;
	};
	inline void operator /= (const T& v) {
		for(unsigned i = 0; i < M; i++) {
			(*this)[i] /= v;
		};
	};

	/// Negation
	inline tThisMatrix operator - () const {
		tThisMatrix m(*this);
		for(unsigned i = 0; i < M; i++) {
			m[i] = -m[i];
		};
		return m;
	};


	/// Comparison
	inline bool operator == (const tThis& m) const {
		for(unsigned i = 0; i < M; i++) {
			if((*this)[i] != m[i]) return false;
		};
		return true;
	};
	inline bool operator != (const tThis& m) const {
		return !(*this == m);
	};
	inline bool operator < (const tThis& m) const {
		for(unsigned i = 0; i < M; i++) {
			if((*this)[i] < m[i]) return true;
		};
		return false;
	};

	inline tRow&	operator [](int i){
		assert(i < M);
		return Value[i];
	};
	inline const tRow&	operator [](int i) const{
		assert(i < M);
		return Value[i];
	};

	inline tThisMatrix inverse() const {
		assert(M == N);
		tThisMatrix m(identity());
		tThis tmp(*this);
		tRow	tmpRow;
		long double	c;
		int		i, j, k, r;

		//
		// Reduce to upper triangular form
		//

		for(i = 0; i < M; i++){

			//
			// Find a row with non-zero pivot value
			//

			for(r = i; r < M; r++){
				if(tmp[r][i] != 0) break;
			};
			if(r >= M) throw this;
			else if(r != i){
				tmpRow = tmp[i];
				tmp[i] = tmp[r];
				tmp[r] = tmpRow;
			};


			//
			// Now eliminate the column
			//

			for(j = i + 1; j < M; j++){
				c = tmp[j][i] / tmp[i][i];
				for(k = 0; k < N; k++){
					tmp[j][k] -= tmp[i][k] * c;
					m[j][k] -= m[i][k] * c;
				};
			};
		};

		//
		// Diagonalize, then reduce to identity
		//

		for(i = M; i > 0; i--){
			for(j = i - 1; j > 0; j--){
				c = tmp[j-1][i-1] / tmp[i-1][i-1];
				for(k = 0; k < N; k++){
					tmp[j-1][k] -= tmp[i-1][k] * c;
					m[j-1][k] -= m[i-1][k] * c;
				};
			};
		};

		for(i = 0; i < M; i++){
			c = 1 / tmp[i][i];
			for(j = 0; j < N; j++){
				m[i][j] *= c;
			};
		};

		return m;
	};

	inline tTranspose transpose() const{
		tTranspose	m;
		for(int i = 0; i < M; i++) for(int j = 0; j < N; j++){
			MatrixTraits<tTranspose::tRow>::RowOf(m[j],i) = MatrixTraits<tThisMatrix::tRow>::RowOf(Value[i], j);
		};
		return m;
	};

	/**
	 * Returns an orthonormal matrix with the same
	 * column space as this matrix.
	 * Gotta love Linear Algebra ;)
	 **/
	inline tThisMatrix orthonormalize(int iter = 8) const{
		Matrix<T,M,N,tThisMatrix> m(*this);
		int			i;

		// Gram-Schmidt couldn't cut it, so lets use Newton :)
		for(i = 0; i < iter; i++){
			m = (m + m.inverse().transpose()) * 0.5;
		};

		return *static_cast<tThisMatrix*>(&m);
	};

	/// Convert to array
	inline T * toArray() {
		return reinterpret_cast<T*>(Value);
	};
	inline const T * toArray() const {
		return reinterpret_cast<const T*>(Value);
	};

	inline static tThisMatrix identity(){
		//
		// Some derived matrix types may initialize with non-zero
		// matrices, so construct the base matrix only then
		// downcast.
		//
		tThis	m;
		unsigned						i;

		for(i = 0; i < M; i++){
			m[i][i] = 1;
		};

		return *static_cast<tThisMatrix*>(&m);
	};
};



//// Vector

template <class T, unsigned Dimension, class Ops>
struct MatrixTraits<Vector<T,Dimension,Ops> > : public MatrixTraits<Matrix<T, Dimension, 1, Ops> > {
};

template <class T, unsigned Dimension> struct VectorOps : public MatrixOps<T, Dimension, 1> {
	typedef VectorOps<T, Dimension> tThisType;
	typedef Vector<T, Dimension, tThisType> tMatrix;
};


/**
 * Vectors, which are special cases of matrices.
 **/
template <class Type, unsigned Dimension, class Ops>
struct Vector : public Matrix<Type, Dimension, 1, Ops>
{
	typedef Vector<Type, Dimension, Ops>	tThisVector;

	Vector() {};
	Vector(Type v1) { (*this)[0] = v1; }
	Vector(Type v1, Type v2) { (*this)[0] = v1; (*this)[1] = v2;}
	Vector(Type v1, Type v2, Type v3) { (*this)[0] = v1; (*this)[1] = v2; (*this)[2] = v3;}
	Vector(Type v1, Type v2, Type v3, Type v4) {
		(*this)[0] = v1; (*this)[1] = v2; (*this)[2] = v3; (*this)[3] = v4;
	}

	/// Implicit conversions
	template <class VT>
	Vector (const VT& v): Matrix<Type, Dimension, 1, Ops>(v) { };

	/// Dot product
	inline Type dot (const tThisVector &v) const{
		return (this->transpose() * v);
	};

	/// Implementation of outer product
	template <class MT>
	struct _Outer {
	};
	template <class _T, unsigned _D, class _Ops>
	struct _Outer<Matrix<_T, _D, 1, _Ops> > {
		typedef Matrix<_T, _D, 1, _Ops> tVector;
		typedef typename MatrixType<Type, Dimension, _D>::tType tReturn;

		inline static tReturn Eval(const tThisVector& a, const tVector& b) {
			return (a * b.transpose());
		};
	};

	/// Outer product
	template <class MT>
	inline typename _Outer<typename MatrixTraits<MT>::tMatrixType>::tReturn	outer(const MT& v) const {
		return typename _Outer<typename MatrixTraits<MT>::tMatrixType>::Eval(*this, v);
	};

	// Cross product
	inline tThisVector cross (const tThisVector &v) const{
		assert (Dimension == 3);
		return tThisVector((this->y() * v.z()) - (this->z() * v.y()), 
			(this->z() * v.x()) - (this->x() * v.z()),
			(this->x() * v.y()) - (this->y() * v.x()));
	};

	/// Projection - yields undefined results if this vector is not normalized
	inline tThisVector project(const tThisVector& v) const{
		return ((*this) * abs(this->dot(v)));
	};

	/// Length/modulus
	inline tScalar length () const {
		return sqrt(lengthSqr());
	};
	inline tScalar lengthSqr () const {
		return dot(*this);
	};

	/// Normalize (make length equal to 1.0)
	inline tThisVector normalize () const {
		// XXX - what if Type != tScalar?
		tThisVector x;
		Type scale = 1.0 / length();

		for(int i=0; i<Dimension; i++) {
			x[i] = (*this)[i] * scale;
		};

		return x;
	};

	inline tThisVector rotate(Type yz, Type xz, Type xy) const {
		// XXX - this does not work properly
		tThisVector v;
		v[0] = (this->x() * (cos(xy) * cos(xz))) + (this->y() * sin(xy)) + (this->z() * sin(xz));
		v[1] = (this->x() * sin(xy)) + (this->y() * (cos(xy) * cos(yz))) + (this->z() * sin(yz));
		v[2] = (this->x() * sin(xz)) + (this->y() * sin(yz)) + (this->z() * (cos(xz) * cos(yz)));
		return v;
	};

	typedef typename Ops::Redim<Dimension,Dimension>::result tHouseholderMatrix;
	inline tHouseholderMatrix householder() const {
		return tHouseholderMatrix::identity() - (outer(*this) * (2 / this->lengthSqr()));
	};

	inline const Type& x() const { return (*this)[0]; };
	inline const Type& y() const { return (*this)[1]; };
	inline const Type& z() const { return (*this)[2]; };

	template <unsigned i>
	inline const Type& get() const {
		return (*this)[i];
	};
	template <unsigned i>
	inline void set(const Type& v) {
		(*this)[i] = v;
	};
};

typedef VectorType<int, 2>::tType iVector2D;
typedef VectorType<float, 2>::tType fVector2D;
typedef VectorType<double, 2>::tType dVector2D;

typedef VectorType<int, 3>::tType iVector3D;
typedef VectorType<float, 3>::tType fVector3D;
typedef VectorType<double, 3>::tType dVector3D;

typedef VectorType<int, 4>::tType iVector4D;
typedef VectorType<float, 4>::tType fVector4D;
typedef VectorType<double, 4>::tType dVector4D;

typedef VectorType<tScalar, 3>::tType tVector;
typedef VectorType<tScalar, 4>::tType tVector4;
typedef VectorType<tScalar, 2>::tType tVector2;

#endif
